In [1]:
# Setup code
%matplotlib inline
import matplotlib.pyplot as plt
import numpy as np
from numerics.matte4.ode import ode_adaptive, ode_solver, implicit_euler
from collections import deque
import numerics.numerical_integration as num_integ
import random

## For animations
from matplotlib import animation, rc
from IPython.display import HTML
##

from numerics.tracker.interpolate_tracker_data import iptrack as trackerData, iptrack_p1100902 as tracker_1, load_p1100902
import numerics.tracker.filter_values as filter_values

TRACK_START_X = 0
TRACK_END_X = 2

run_slow = False

Testing the code with simulations.

Loading a track

Displays a track polynomial from tracker data

In [2]:
def trackLoading():
    (ball_x, ball_y), polynomial, track_end_x = tracker_1() #trackerData("test_data/mass_a.txt")
    steps = 100
    x = np.linspace(0, track_end_x, steps)
    y = np.polyval(polynomial, x)
    
    plt.figure(figsize=(12,6))
    plt.plot(x, y, linewidth=2, label="Track")
    plt.scatter(ball_x, ball_y, label="Ball start")
    plt.legend()
    plt.show()

trackLoading()

Animating a position over a simple curve

Let's try animation with a very simple parabola, $x^2 - 1.5x + 0.57$

In [3]:
# Just showing the curve
def simpleCurve():
    parabola = [1, -1.5, 0.57]
    d_parabola = np.polyder(parabola)

    steps = 100
    framerate = 20/1000
    frame_count = 300
    

    x_pos = [0]
    vel = [0]
    for i in range(frame_count-1):
        time = i*framerate
        
        # Bruker matte4-funksjoner, heh
        # part 1
        # v'=sin(alpha)*9.81
        # where functions ode_adaptive ask for x, think t for time instead.
        current_x = x_pos[-1]
        def accell(t, v):
            track_angle = np.arctan(-np.polyval(d_parabola, current_x))
            return np.sin(track_angle)*9.81 - 0.8*vel[-1]
        
        t_num, v_num = ode_adaptive(accell, x0=0, xend=framerate, y0=vel[-1], h0=0.2)
        vel.append(v_num[-1])
        
        #part 2
        def velocity(t, x):
            return v_num[-1]
        
        t_num, x_num = ode_adaptive(velocity, x0=0, xend=framerate, y0=current_x, h0=0.2)
        x_pos.append(x_num[-1])
        # END matte 4
    
    x = np.linspace(0, 1.5, steps)
    y = np.polyval(parabola, x)
    
    time_axis = np.linspace(0, frame_count*framerate, frame_count)
    
    # Plot
    fig, (x_ax, t_ax) = plt.subplots(2, figsize=[12, 8])
    x_ax.plot(x, y, label=f"{parabola[0]}x^2 + {parabola[1]}x + {parabola[2]}")
    x_ax.plot(x, np.arctan(-np.polyval(d_parabola, x)), label="Track Angle (rad)")
    x_ax.plot(x, -np.polyval(d_parabola, x), label="Track derivative (negated)")
    x_ax.plot(x, np.sin(np.arctan(-np.polyval(d_parabola, x))), label="Track Angle cosine")
    x_ax.legend()
    
    t_ax.plot(time_axis, vel, label="Ball velocity")
    t_ax.plot(time_axis, x_pos, label="Ball x")
    t_ax.legend()
    
    #plt.show()

simpleCurve()
C:\Users\krire\OneDrive\Documents\MTDT\19V\Fysikk\Numerikk\numerics\matte4\ode.py:121: RuntimeWarning: divide by zero encountered in double_scalars
  h = 0.8*(tol/error_estimate)**(1/(p+1))*h
In [4]:
def curveAnimation():
    fig, ax = plt.subplots()

    plt.xlim((0, 1.5))
    # Background path using the curve
    parabola = [1, -1.5, 0.57]
    d_parabola = np.polyder(parabola)
    steps = 100
    x = np.linspace(0, 1.5, steps)
    y = np.polyval(parabola, x)
    plt.plot(x, y, label=f"{parabola[0]}x^2 + {parabola[1]}x + {parabola[2]}")
    plt.legend()
    
    # Animated line
    ax.set_xlim(( 0, 1.5))
    ax.set_ylim((0, 1))
    line, = ax.plot([], [], linestyle='None', marker="o", label="Frame: 0")
    ax.legend()
    
    # Animation settings
    framerate = 16/1000
    frame_count = 350
    print(f"Total duration of animation is {framerate*frame_count}s")

    def init():
        line.set_data([], [])
        return (line,)
        
    x_pos = [0]
    y_pos = [np.polyval(parabola, x_pos[0])]
    vel = [0]
        
    def animate(frame):
        # Bruker matte4-funksjoner, heh
        
        # part 1 - integrate acceleration to get velocity
        # v'=sin(alpha)*9.81
        # where functions ode_adaptive ask for x, think t for time instead.
        current_x = x_pos[-1]
        def accell(t, v):
            track_angle = np.arctan(-np.polyval(d_parabola, current_x))
            return np.sin(track_angle)*9.81 - 0.8*vel[-1]
        
        t_num, v_num = ode_adaptive(accell, x0=0, xend=framerate, y0=vel[-1], h0=framerate/2)
        # Save velocity
        vel.append(v_num[-1])
        
        #part 2 - integrate velocity to get position
        def velocity(t, x):
            return v_num[-1]
        
        t_num, x_num = ode_adaptive(velocity, x0=0, xend=framerate, y0=current_x, h0=framerate)
        
        # Save the values
        x_pos.append(x_num[-1])
        y_pos.append(np.polyval(parabola, x_num[-1]))
        
        # Update the animated point
        line.set_data(x_pos[-1], y_pos[-1])
        line.set_label(f"Frame {frame}, t={np.round(frame*framerate, 1)}. pos=({round(x_pos[-1],1)}, {round(y_pos[-1],1)}) ")
        ax.legend()
        return (line,) # Return all updated artists

    anim = animation.FuncAnimation(fig, animate, init_func=init,
                                   frames=frame_count, interval=framerate*1000, 
                                   blit=True)
    rc('animation', html='jshtml')
    return anim

anim = curveAnimation()
save_vid = False
if save_vid:
    # Set up formatting for the movie files
    Writer = animation.writers['ffmpeg']
    writer = Writer(fps=1/framerate, metadata=dict(artist='Kristian'), bitrate=1800)
    anim.save('out/1-simple-curve-ball.mp4', writer=writer)
    print("Saved!")

if run_slow:
    anim
Total duration of animation is 5.6000000000000005s
In [5]:
# Curve with obstacle animation
def obstacleCurveAnimation(parabola):
    fig, ax = plt.subplots()

    plt.xlim((TRACK_START_X, TRACK_END_X))
    # Background path using the curve
    d_parabola = np.polyder(parabola)
    steps = 100
    x = np.linspace(TRACK_START_X, TRACK_END_X, steps)
    y = np.polyval(parabola, x)
    label = " ".join([f"{round(c, 1)}x^{len(parabola)-i}" for i, c in enumerate(parabola)])
    plt.plot(x, y, label=label)
    plt.legend()
    
    # Animated line
    ax.set_xlim((TRACK_START_X, TRACK_END_X))
    ax.set_ylim((-0.2, 1))
    line, = ax.plot([], [], linestyle='None', marker="o", label="Frame: 0")
    ax.legend()
    
    # Animation settings
    framerate = 16/1000
    frame_count = 350
    print(f"Total duration of animation is {framerate*frame_count}s")

    def init():
        line.set_data([], [])
        return (line,)
        
    x_pos = [0]
    y_pos = [np.polyval(parabola, x_pos[0])]
    vel = [0]
        
    def animate(frame):
        # Bruker matte4-funksjoner, heh
        
        # part 1 - integrate acceleration to get velocity
        # v'=sin(alpha)*9.81
        # where functions ode_adaptive ask for x, think t for time instead.
        current_x = x_pos[-1]
        track_angle = np.arctan(-np.polyval(d_parabola, current_x))

        def accell(t, v):
            gravity = 9.81
            braking_factor = 0.4
            return (5/7)*gravity*np.sin(track_angle) - braking_factor*v
        
        t_num, v_num = ode_adaptive(accell, x0=0, xend=framerate, y0=vel[-1], h0=framerate/2)
        # Save velocity
        vel.append(v_num[-1])
        
        #part 2 - integrate velocity to get position
        def velocity(t, x):
            angle = np.arctan(-np.polyval(d_parabola, x))
            return v_num[-1]*np.cos(angle)
        
        t_num, x_num = ode_adaptive(velocity, x0=0, xend=framerate, y0=current_x, h0=framerate)
        
        # Save the values
        x_pos.append(x_num[-1])
        y_pos.append(np.polyval(parabola, x_num[-1]))
        
        # Update the animated point
        line.set_data(x_pos[-1], y_pos[-1])
        line.set_label(f"Frame {frame}, t={np.round(frame*framerate, 1)}. pos=({round(x_pos[-1],1)}, {round(y_pos[-1],1)}) ")
        ax.legend()
        return (line,) # Return all updated artists
    
    anim = animation.FuncAnimation(fig, animate, init_func=init,
                                   frames=frame_count, interval=framerate*1000, 
                                   blit=True)
    rc('animation', html='jshtml')
    return anim
In [6]:
# Running the animation function
def showAnimation():
    parabola = [3, -11.3, 14.08, -6.508, 1.0768]
    anim = obstacleCurveAnimation(parabola)
    save_vid = False
    if save_vid:
        # Set up formatting for the movie files
        Writer = animation.writers['ffmpeg']
        writer = Writer(fps=1/framerate, metadata=dict(artist='Kristian'), bitrate=1800)
        anim.save('out/1-simple-curve-ball.mp4', writer=writer)
        print("Saved!")
    return anim

print("Wait a few seconds for the animation to show")
anim = showAnimation()
if run_slow:
    anim
Wait a few seconds for the animation to show
Total duration of animation is 5.6000000000000005s

Justerbare polynomer

Vi trenger et uttrykk som kan gi polynomer ut i fra høyden på midten.

In [7]:
def makePolynom(height):
    points = [(0, 1), (0.5, 0), (1, height), (1.5, 0), (2, 1)]
    x = [p[0] for p in points]
    y = [p[1] for p in points]
    return np.polyfit(x, y, len(points)-1)

def plotPolynomer():
    steps = 100
    x = np.linspace(TRACK_START_X, TRACK_END_X, steps)
    heights = [-0.3, 0, 0.1, 0.3, 0.5]
    polynoms = [makePolynom(h) for h in heights]
    
    plt.xlim((0, 2))
    for i, p in enumerate(polynoms):
        y = np.polyval(p, x)
        plt.plot(x, y, label=f"Height {heights[i]}")
    
    plt.legend()
    plt.show()

plotPolynomer()

Animating with different parabola heights

In [8]:
print("Wait a few seconds for the animation to show")
if run_slow:
    obstacleCurveAnimation(makePolynom(0.2))
Wait a few seconds for the animation to show

Detecting passing the obstacle

a passing of the obstacle could be detected by looking at $x(t_1), x(t_2), v(t_1)$ and verifying that $x$ moved past the point $x=x_{obstacle}$ (test with $ x_{obstacle} \in \left(x(t_1), x(t_2)\right) $) while $v$ was positive for $x(t_1) < x_{obstacle}$ and negative for $x(t_1) > x_{obstacle}$.

In [9]:
# Loop over and analyse x and v
def makePasses():
    parabola = makePolynom(0.2)
    d_parabola = np.polyder(parabola)

    steps = 100
    framerate = 20/1000
    frame_count = 300
    
    t = [0]
    x_pos = [0]
    vel = [0]
    for i in range(frame_count-1):
        time = i*framerate

        t.append(time)
        current_x = x_pos[-1]
        def accell(t, v):
            gravity = 9.81
            braking_factor = 0.4
            track_angle = np.arctan(-np.polyval(d_parabola, current_x))
            return (5/7)*gravity*np.sin(track_angle) - braking_factor*v
        
        t_num, v_num = ode_adaptive(accell, x0=0, xend=framerate, y0=vel[-1], h0=0.2)
        vel.append(v_num[-1])
        
        #part 2
        def velocity(t, x):
            angle = np.arctan(-np.polyval(d_parabola, x))
            return v_num[-1]*np.cos(angle)
        
        t_num, x_num = ode_adaptive(velocity, x0=0, xend=framerate, y0=current_x, h0=0.2)
        x_pos.append(x_num[-1])
        # END matte 4
    
    x = np.linspace(0, 1.5, steps)
    y = np.polyval(parabola, x)
    
    time_axis = np.linspace(0, frame_count*framerate, frame_count)
    
    # Plot
    fig, (x_ax, x_ax2, t_ax) = plt.subplots(3, figsize=[12, 8])
    x_ax.plot(x, y, label=f"{parabola[0]}x^2 + {parabola[1]}x + {parabola[2]}")
    x_ax.plot(x, np.arctan(-np.polyval(d_parabola, x)), label="Track Angle (rad)")
    x_ax.plot(x, np.sin(np.arctan(-np.polyval(d_parabola, x))), label="Track Angle cosine")
    x_ax.legend()
    
    x_ax2.plot(x, -np.polyval(d_parabola, x), label="Track derivative (negated)")
    x_ax2.legend()
    
    t_ax.plot(time_axis, vel, label="Ball velocity")
    t_ax.plot(time_axis, x_pos, label="Ball x")
    t_ax.axhline(1, label="x_obstacle", color="orange")
    t_ax.legend()
    
    return (t, x_pos, vel)

def contains(a, b, obstacle):
    return (a <= obstacle <= b) or (b <= obstacle <= a)

def detectPasses(times, x, velocities, obstacle_x):
    to_chunk = deque(enumerate(velocities)) # [(index, velocity)]
    chunks = [[]]
    current_sign = np.sign([vel[1] for vel in to_chunk if vel[1] != 0][0])
    while len(to_chunk) > 0:
        i, v = to_chunk.popleft()
        sign = np.sign(v)
        if sign != current_sign and sign != 0:
            current_sign = sign
            chunks.append([])
        chunks[-1].append((i, v, times[i]))
    #print(f"Got {len(chunks)} chunks where speed is the same direction")
    
    obstacle_passes = [chunk for chunk in chunks if len(chunk) > 2 and contains(x[chunk[0][0]], x[chunk[-1][0]], obstacle_x)]
    return obstacle_passes

(t, x, v) = makePasses()
obstacle_x = 1
passes = detectPasses(t, x, v, obstacle_x)
for i, chunk in enumerate(passes):
    print(f"Pass {i}:")
    for j, vel, time in chunk:
        print(f"{j}\t{time}\t{vel}")
Pass 0:
0	0	0
1	0.0	0.13759913411744748
2	0.02	0.27409482278754893
3	0.04	0.409488744158406
4	0.06	0.5437823635438759
5	0.08	0.6769767985375327
6	0.1	0.8090726716070352
7	0.12	0.9400699432528925
8	0.14	1.0699677170776454
9	0.16	1.198764005772717
10	0.18	1.3264554434415954
11	0.2	1.4530369244126111
12	0.22	1.5785011408919722
13	0.24	1.7028379801179525
14	0.26	1.8260337237105608
15	0.28	1.9480699642966257
16	0.3	2.068922109965871
17	0.32	2.1885572747159174
18	0.34	2.3069312308727667
19	0.36	2.4239838866582684
20	0.38	2.539632366914284
21	0.4	2.653760046871797
22	0.42	2.7661984426342214
23	0.44	2.8766958184450266
24	0.46	2.9848595263851756
25	0.48	3.090042479681862
26	0.5	3.191100247971163
27	0.52	3.285819108078552
28	0.54	3.369441700749603
29	0.56	3.4309370914971105
30	0.58	3.4490826316744005
31	0.6	3.41296120308771
32	0.62	3.3414181866706016
33	0.64	3.253375415549101
34	0.66	3.1591601174709
35	0.68	3.0647172815918036
36	0.7000000000000001	2.9743179391885195
37	0.72	2.8917923081617354
38	0.74	2.8210463179685417
39	0.76	2.766019788154122
40	0.78	2.7299850085488337
41	0.8	2.7143870510250783
42	0.8200000000000001	2.718136616305175
43	0.84	2.7381119463156263
44	0.86	2.770352716704744
45	0.88	2.810953865915093
46	0.9	2.8563342219992705
47	0.92	2.903040321798631
48	0.9400000000000001	2.9472076003606245
49	0.96	2.9835425459833083
50	0.98	3.0033980231672652
51	1.0	2.992330331224692
52	1.02	2.9353226490528805
53	1.04	2.836582494518996
54	1.06	2.7136537623335864
55	1.08	2.578630880989184
56	1.1	2.4374757843982864
57	1.12	2.293125534029907
58	1.1400000000000001	2.1471395521801524
59	1.16	2.0004128963672767
60	1.18	1.8534945340091535
61	1.2	1.7067398659577926
62	1.22	1.5603892891073126
63	1.24	1.4146113462618857
64	1.26	1.2695277744265043
65	1.28	1.1252287534177416
66	1.3	0.9817825795699547
67	1.32	0.8392420302369477
68	1.34	0.697648691488918
69	1.36	0.5570359935717867
70	1.3800000000000001	0.4174314064070779
71	1.4000000000000001	0.27885807966117004
72	1.42	0.14133611294522397
73	1.44	0.004883582041387935
Pass 1:
74	1.46	-0.1304825890881951
75	1.48	-0.2647458421175675
76	1.5	-0.3978892269520814
77	1.52	-0.5298946198894907
78	1.54	-0.6607418611871017
79	1.56	-0.7904077454798852
80	1.58	-0.9188647788347767
81	1.6	-1.0460795828232368
82	1.62	-1.1720107712686374
83	1.6400000000000001	-1.2966060355523008
84	1.6600000000000001	-1.4197980242183232
85	1.68	-1.5414983458030609
86	1.7	-1.6615885694674644
87	1.72	-1.7799062651481496
88	1.74	-1.8962225337438714
89	1.76	-2.0102042954957176
90	1.78	-2.12134791758261
91	1.8	-2.228855990709435
92	1.82	-2.3313950664613823
93	1.84	-2.426593871485593
94	1.86	-2.509989877091201
95	1.8800000000000001	-2.573143244650883
96	1.9000000000000001	-2.602961118944731
97	1.92	-2.590765393900857
98	1.94	-2.543723991603616
99	1.96	-2.4752408760639315
100	1.98	-2.3951474627648865
101	2.0	-2.309423332266119
102	2.02	-2.221821254880624
103	2.04	-2.1349440604454633
104	2.06	-2.0508183051542974
105	2.08	-1.9711953883870366
106	2.1	-1.8977068339607257
107	2.12	-1.8319283904442092
108	2.14	-1.7753721498187447
109	2.16	-1.729413453816009
110	2.18	-1.695165116038782
111	2.2	-1.6733310046538452
112	2.22	-1.6640905144918758
113	2.24	-1.667064432499279
114	2.2600000000000002	-1.6813783345793198
115	2.2800000000000002	-1.7057977000470725
116	2.3000000000000003	-1.7388826725621793
117	2.32	-1.779117007326685
118	2.34	-1.8249884635700615
119	2.36	-1.8750176625558388
120	2.38	-1.9277407161393643
121	2.4	-1.9816486381864524
122	2.42	-2.035076563465714
123	2.44	-2.0860183568788844
124	2.46	-2.131814226667242
125	2.48	-2.1686246493219574
126	2.5	-2.190638843734861
127	2.52	-2.1894930731596127
128	2.54	-2.1563115577649477
129	2.56	-2.088727407812491
130	2.58	-1.993998983779175
131	2.6	-1.881888884317
132	2.62	-1.7594480258342609
133	2.64	-1.630903688964807
134	2.66	-1.4987288424233614
135	2.68	-1.364414671533083
136	2.7	-1.2289010322100034
137	2.72	-1.092807270057041
138	2.74	-0.9565586370473959
139	2.7600000000000002	-0.8204582339891693
140	2.7800000000000002	-0.684729706850631
141	2.8000000000000003	-0.5495436598344012
142	2.82	-0.4150346739281273
143	2.84	-0.28131273743706287
144	2.86	-0.14847128092793824
145	2.88	-0.016593138457937806
Pass 2:
146	2.9	0.11424472111618258
147	2.92	0.24396710933365767
148	2.94	0.37249672563733505
149	2.96	0.4997500412072147
150	2.98	0.6256325027740558
151	3.0	0.7500324696096493
152	3.02	0.8728130380333601
153	3.04	0.9938004393246148
154	3.06	1.1127668583794317
155	3.08	1.2294039997710497
156	3.1	1.343280909923884
157	3.12	1.45377423943019
158	3.14	1.5599490290074067
159	3.16	1.6603497369407045
160	3.18	1.7526340329186458
161	3.2	1.8329784616580318
162	3.22	1.8954061417521655
163	3.24	1.932207676587433
164	3.2600000000000002	1.9377903733622717
165	3.2800000000000002	1.9136699539485995
166	3.3000000000000003	1.8669449684166215
167	3.3200000000000003	1.8051177029748593
168	3.34	1.7338090748771293
169	3.36	1.6568313490036215
170	3.38	1.576758912237465
171	3.4	1.4953846331612688
172	3.42	1.41400934932902
173	3.44	1.3336156318288748
174	3.46	1.254971307828278
175	3.48	1.1786914433386773
176	3.5	1.1052755906201024
177	3.52	1.0351302250861762
178	3.54	0.9685824273509682
179	3.56	0.9058886497603766
180	3.58	0.847241061907275
181	3.6	0.7927730744869532
182	3.62	0.7425649861792284
183	3.64	0.6966501981476099
184	3.66	0.655022069708988
185	3.68	0.6176412398182902
186	3.7	0.5844431059559783
187	3.72	0.5553451164892437
188	3.74	0.5302535680924272
189	3.7600000000000002	0.5090696722828407
190	3.7800000000000002	0.4916947440523876
191	3.8000000000000003	0.4780344461937207
192	3.8200000000000003	0.46800208681360755
193	3.84	0.4615210003729744
194	3.86	0.45852594726870416
195	3.88	0.4589642777573026
196	3.9	0.46279557841061253
197	3.92	0.4699910960518913
198	3.94	0.4805324493130855
199	3.96	0.4944097141513882
200	3.98	0.5116189055150507
201	4.0	0.532158890376479
202	4.0200000000000005	0.5560277643172056
203	4.04	0.583218757972471
204	4.0600000000000005	0.6137157670794772
205	4.08	0.647488636967955
206	4.1	0.684488367512011
207	4.12	0.7246424225940059
208	4.14	0.7678503261313027
209	4.16	0.8139796877701704
210	4.18	0.8628627203196874
211	4.2	0.9142931827961441
212	4.22	0.9680235113217828
213	4.24	1.0237616813720543
214	4.26	1.0811670810515435
215	4.28	1.1398443410178214
216	4.3	1.1993336162284844
217	4.32	1.259095150578574
218	4.34	1.3184849064678248
219	4.36	1.3767163636688196
220	4.38	1.4328008838176582
221	4.4	1.485454903444772
222	4.42	1.532957458153682
223	4.44	1.572940976242675
224	4.46	1.602124098954387
225	4.48	1.6161162831371172
226	4.5	1.6097559294997792
227	4.5200000000000005	1.5787673147380108
228	4.54	1.5224121351993727
229	4.5600000000000005	1.4442678424119553
230	4.58	1.3498950749322594
231	4.6000000000000005	1.2443584583228802
232	4.62	1.1313880012805415
233	4.64	1.0135258690660287
234	4.66	0.892480878965986
235	4.68	0.7694188372674404
236	4.7	0.6451570796248571
237	4.72	0.5202878850348105
238	4.74	0.3952565262173394
239	4.76	0.2704116544749234
240	4.78	0.14603900870778594
241	4.8	0.02238515598644146

Using our integrator class

In [10]:
def useClass():
    ball = num_integ.Ball(mass=0.01, radius=0.01, start_x=0, start_y=0.7)
    polynom = makePolynom(0.25)
    track = num_integ.Track(polynom, start_height=1, obstacle_height=0.25, end_height=1)
    
    integrator = num_integ.BallIntegrator(ball, track)
    integrator.timestep = 0.016
    integrator.braking_factor=0.8
    
    steps = 600
    print(f"Calculating to time {steps*integrator.timestep}s")
    for step in range(steps):
        integrator.step()
    
    forceCalculator = num_integ.ForceCalculator(track, ball, integrator.time, integrator.position, integrator.velocity, integrator.acceleration)
    passChecker = num_integ.ObstaclePassChecker(obstacle_x=1) # x defined in makePolynom
    frictions, normalForces = forceCalculator.calculate()
    
    flight = None
    for i, n in enumerate(normalForces):
        if n < 0:
            flight = integrator.position[i]
            break
    
    # Plot stuff
    fig, (ax0, ax1, ax2) = plt.subplots(3, figsize=(17, 9))
    x = np.linspace(0, 2, 100)
    ax0.plot(x, np.polyval(polynom, x))
    if flight:
        ax0.axvline(x=flight)
    
    ax1.plot(integrator.time, integrator.position, label="Position", color="red")
    ax1.plot(integrator.time, integrator.velocity, label="Velocity")
    ax1.plot(integrator.time, integrator.acceleration, label="Acceleration")
    ax1.axhline(1, label="x_obstacle", color="red")
    ax1.legend()
    
    ax2.plot(integrator.time, frictions, label="Friction")
    ax2.plot(integrator.time, normalForces, label="Normal force")
    ax2.legend()


useClass()
Calculating to time 9.6s

Testing multiple parabolas for 2 passes

In [11]:
# TODO
(t, x, v) = makePasses()
obstacle_x = 1
passes = detectPasses(t, x, v, obstacle_x)
for i, chunk in enumerate(passes):
    print(f"Pass {i}:")
    for j, vel, time in chunk:
        print(f"{j}\t{time}\t{vel}")
Pass 0:
0	0	0
1	0.0	0.13759913411744748
2	0.02	0.27409482278754893
3	0.04	0.409488744158406
4	0.06	0.5437823635438759
5	0.08	0.6769767985375327
6	0.1	0.8090726716070352
7	0.12	0.9400699432528925
8	0.14	1.0699677170776454
9	0.16	1.198764005772717
10	0.18	1.3264554434415954
11	0.2	1.4530369244126111
12	0.22	1.5785011408919722
13	0.24	1.7028379801179525
14	0.26	1.8260337237105608
15	0.28	1.9480699642966257
16	0.3	2.068922109965871
17	0.32	2.1885572747159174
18	0.34	2.3069312308727667
19	0.36	2.4239838866582684
20	0.38	2.539632366914284
21	0.4	2.653760046871797
22	0.42	2.7661984426342214
23	0.44	2.8766958184450266
24	0.46	2.9848595263851756
25	0.48	3.090042479681862
26	0.5	3.191100247971163
27	0.52	3.285819108078552
28	0.54	3.369441700749603
29	0.56	3.4309370914971105
30	0.58	3.4490826316744005
31	0.6	3.41296120308771
32	0.62	3.3414181866706016
33	0.64	3.253375415549101
34	0.66	3.1591601174709
35	0.68	3.0647172815918036
36	0.7000000000000001	2.9743179391885195
37	0.72	2.8917923081617354
38	0.74	2.8210463179685417
39	0.76	2.766019788154122
40	0.78	2.7299850085488337
41	0.8	2.7143870510250783
42	0.8200000000000001	2.718136616305175
43	0.84	2.7381119463156263
44	0.86	2.770352716704744
45	0.88	2.810953865915093
46	0.9	2.8563342219992705
47	0.92	2.903040321798631
48	0.9400000000000001	2.9472076003606245
49	0.96	2.9835425459833083
50	0.98	3.0033980231672652
51	1.0	2.992330331224692
52	1.02	2.9353226490528805
53	1.04	2.836582494518996
54	1.06	2.7136537623335864
55	1.08	2.578630880989184
56	1.1	2.4374757843982864
57	1.12	2.293125534029907
58	1.1400000000000001	2.1471395521801524
59	1.16	2.0004128963672767
60	1.18	1.8534945340091535
61	1.2	1.7067398659577926
62	1.22	1.5603892891073126
63	1.24	1.4146113462618857
64	1.26	1.2695277744265043
65	1.28	1.1252287534177416
66	1.3	0.9817825795699547
67	1.32	0.8392420302369477
68	1.34	0.697648691488918
69	1.36	0.5570359935717867
70	1.3800000000000001	0.4174314064070779
71	1.4000000000000001	0.27885807966117004
72	1.42	0.14133611294522397
73	1.44	0.004883582041387935
Pass 1:
74	1.46	-0.1304825890881951
75	1.48	-0.2647458421175675
76	1.5	-0.3978892269520814
77	1.52	-0.5298946198894907
78	1.54	-0.6607418611871017
79	1.56	-0.7904077454798852
80	1.58	-0.9188647788347767
81	1.6	-1.0460795828232368
82	1.62	-1.1720107712686374
83	1.6400000000000001	-1.2966060355523008
84	1.6600000000000001	-1.4197980242183232
85	1.68	-1.5414983458030609
86	1.7	-1.6615885694674644
87	1.72	-1.7799062651481496
88	1.74	-1.8962225337438714
89	1.76	-2.0102042954957176
90	1.78	-2.12134791758261
91	1.8	-2.228855990709435
92	1.82	-2.3313950664613823
93	1.84	-2.426593871485593
94	1.86	-2.509989877091201
95	1.8800000000000001	-2.573143244650883
96	1.9000000000000001	-2.602961118944731
97	1.92	-2.590765393900857
98	1.94	-2.543723991603616
99	1.96	-2.4752408760639315
100	1.98	-2.3951474627648865
101	2.0	-2.309423332266119
102	2.02	-2.221821254880624
103	2.04	-2.1349440604454633
104	2.06	-2.0508183051542974
105	2.08	-1.9711953883870366
106	2.1	-1.8977068339607257
107	2.12	-1.8319283904442092
108	2.14	-1.7753721498187447
109	2.16	-1.729413453816009
110	2.18	-1.695165116038782
111	2.2	-1.6733310046538452
112	2.22	-1.6640905144918758
113	2.24	-1.667064432499279
114	2.2600000000000002	-1.6813783345793198
115	2.2800000000000002	-1.7057977000470725
116	2.3000000000000003	-1.7388826725621793
117	2.32	-1.779117007326685
118	2.34	-1.8249884635700615
119	2.36	-1.8750176625558388
120	2.38	-1.9277407161393643
121	2.4	-1.9816486381864524
122	2.42	-2.035076563465714
123	2.44	-2.0860183568788844
124	2.46	-2.131814226667242
125	2.48	-2.1686246493219574
126	2.5	-2.190638843734861
127	2.52	-2.1894930731596127
128	2.54	-2.1563115577649477
129	2.56	-2.088727407812491
130	2.58	-1.993998983779175
131	2.6	-1.881888884317
132	2.62	-1.7594480258342609
133	2.64	-1.630903688964807
134	2.66	-1.4987288424233614
135	2.68	-1.364414671533083
136	2.7	-1.2289010322100034
137	2.72	-1.092807270057041
138	2.74	-0.9565586370473959
139	2.7600000000000002	-0.8204582339891693
140	2.7800000000000002	-0.684729706850631
141	2.8000000000000003	-0.5495436598344012
142	2.82	-0.4150346739281273
143	2.84	-0.28131273743706287
144	2.86	-0.14847128092793824
145	2.88	-0.016593138457937806
Pass 2:
146	2.9	0.11424472111618258
147	2.92	0.24396710933365767
148	2.94	0.37249672563733505
149	2.96	0.4997500412072147
150	2.98	0.6256325027740558
151	3.0	0.7500324696096493
152	3.02	0.8728130380333601
153	3.04	0.9938004393246148
154	3.06	1.1127668583794317
155	3.08	1.2294039997710497
156	3.1	1.343280909923884
157	3.12	1.45377423943019
158	3.14	1.5599490290074067
159	3.16	1.6603497369407045
160	3.18	1.7526340329186458
161	3.2	1.8329784616580318
162	3.22	1.8954061417521655
163	3.24	1.932207676587433
164	3.2600000000000002	1.9377903733622717
165	3.2800000000000002	1.9136699539485995
166	3.3000000000000003	1.8669449684166215
167	3.3200000000000003	1.8051177029748593
168	3.34	1.7338090748771293
169	3.36	1.6568313490036215
170	3.38	1.576758912237465
171	3.4	1.4953846331612688
172	3.42	1.41400934932902
173	3.44	1.3336156318288748
174	3.46	1.254971307828278
175	3.48	1.1786914433386773
176	3.5	1.1052755906201024
177	3.52	1.0351302250861762
178	3.54	0.9685824273509682
179	3.56	0.9058886497603766
180	3.58	0.847241061907275
181	3.6	0.7927730744869532
182	3.62	0.7425649861792284
183	3.64	0.6966501981476099
184	3.66	0.655022069708988
185	3.68	0.6176412398182902
186	3.7	0.5844431059559783
187	3.72	0.5553451164892437
188	3.74	0.5302535680924272
189	3.7600000000000002	0.5090696722828407
190	3.7800000000000002	0.4916947440523876
191	3.8000000000000003	0.4780344461937207
192	3.8200000000000003	0.46800208681360755
193	3.84	0.4615210003729744
194	3.86	0.45852594726870416
195	3.88	0.4589642777573026
196	3.9	0.46279557841061253
197	3.92	0.4699910960518913
198	3.94	0.4805324493130855
199	3.96	0.4944097141513882
200	3.98	0.5116189055150507
201	4.0	0.532158890376479
202	4.0200000000000005	0.5560277643172056
203	4.04	0.583218757972471
204	4.0600000000000005	0.6137157670794772
205	4.08	0.647488636967955
206	4.1	0.684488367512011
207	4.12	0.7246424225940059
208	4.14	0.7678503261313027
209	4.16	0.8139796877701704
210	4.18	0.8628627203196874
211	4.2	0.9142931827961441
212	4.22	0.9680235113217828
213	4.24	1.0237616813720543
214	4.26	1.0811670810515435
215	4.28	1.1398443410178214
216	4.3	1.1993336162284844
217	4.32	1.259095150578574
218	4.34	1.3184849064678248
219	4.36	1.3767163636688196
220	4.38	1.4328008838176582
221	4.4	1.485454903444772
222	4.42	1.532957458153682
223	4.44	1.572940976242675
224	4.46	1.602124098954387
225	4.48	1.6161162831371172
226	4.5	1.6097559294997792
227	4.5200000000000005	1.5787673147380108
228	4.54	1.5224121351993727
229	4.5600000000000005	1.4442678424119553
230	4.58	1.3498950749322594
231	4.6000000000000005	1.2443584583228802
232	4.62	1.1313880012805415
233	4.64	1.0135258690660287
234	4.66	0.892480878965986
235	4.68	0.7694188372674404
236	4.7	0.6451570796248571
237	4.72	0.5202878850348105
238	4.74	0.3952565262173394
239	4.76	0.2704116544749234
240	4.78	0.14603900870778594
241	4.8	0.02238515598644146

Running with real data

In [12]:
def count_passes_track1():
    data = load_p1100902()
    data = filter_values.calculate_track_values(data)
    t = data[:,0]
    x = data[:,1]
    velocities = data[:,3]
    accelerations = data[:,4]
    
    fig, (plt1, plt2, plt3) = plt.subplots(3, 1, figsize=(12,9))
    #plt1.set_xlim(0, 2.7)
    #plt2.set_xlim(0, 2.7)
    #plt3.set_xlim(0, 2.7)
    
    plt1.plot(t, x, label="Measured position")
    plt2.plot(t, velocities, label="Measured velocity")
    plt3.plot(t, accelerations, label="Measured acceleration")
    plt1.legend()
    plt2.legend()
    plt3.legend()
    plt.show()
    
    obstacle_x = 0.59890243
    passes = detectPasses(t, x, velocities, obstacle_x)
    print(f"Found {len(passes)} passes")
    #for i, chunk in enumerate(passes):
    #    print(f"Pass {i}:")
    #    for j, vel, time in chunk:
    #        print(f"{j}\t{time}\t{vel}")

count_passes_track1()
Found 2 passes
In [13]:
def useReal():
    measured_data = load_p1100902() # For comparison

    (ball_x, ball_y), polynom, track_end_x = tracker_1()
    obstacle_x = 0.59890243
    #print(f"Roots at {np.roots(np.polyder(polynom))}")
    print(f"Obstacle at {obstacle_x}")
    
    ball = num_integ.Ball(mass=0.05, radius=0.0189/2, start_x=ball_x, start_y=ball_y)
    track = num_integ.Track(polynom, start_height=0.3, obstacle_height=0.238, end_height=0.3)
    
    integrator = num_integ.BallIntegrator(ball, track, braking_factor=0.11097230149082478) # Found later
    integrator.timestep = 0.01
    
    steps = 2000
    print(f"Calculating to time {steps*integrator.timestep}s")
    for step in range(steps):
        integrator.step()
    
    forceCalculator = num_integ.ForceCalculator(track, ball, integrator.time, integrator.position, integrator.velocity, integrator.acceleration)
    passChecker = num_integ.ObstaclePassChecker(obstacle_x=1) # x defined in makePolynom
    frictions, normalForces = forceCalculator.calculate()
    
    flight = None
    for i, n in enumerate(normalForces):
        if n < 0:
            flight = integrator.position[i]
            break
    
    passes = detectPasses(integrator.time, integrator.position, integrator.velocity, obstacle_x)
    print(f"Found {len(passes)} passes")
    
    # Plot stuff
    fig, (ax0, ax1, ax2, ax3) = plt.subplots(4, figsize=(17, 12))
    x = np.linspace(0, track_end_x, 100)
    ax0.plot(x, np.polyval(polynom, x), label="Track")
    ax0.axvline(obstacle_x, label="Obstacle", color="red")
    ax0.legend()
    if flight:
        ax0.axvline(x=flight, label="Error! Liftoff/flight here")
    
    ax1.plot(integrator.time, integrator.position, label="Position", color="red")
    ax1.plot(integrator.time, integrator.velocity, label="Velocity")
    ax1.plot(integrator.time, integrator.acceleration, label="Acceleration")
    ax1.axhline(obstacle_x, label="x_obstacle", color="red")
    ax1.legend()
    
    ax2.plot(integrator.time, frictions, label="Friction")
    ax2.plot(integrator.time, normalForces, label="Normal force")
    ax2.legend()
    
    ax3.plot(measured_data[:,0], measured_data[:,1], label="Measured position", color="black")
    ax3.plot(integrator.time, integrator.position, label="Position", color="red")
    ax3.legend()

useReal()
Obstacle at 0.59890243
Calculating to time 20.0s
Found 2 passes

Trying to find the closest match for braking factor

In [14]:
def calculate_measured_values(obstacle_x = 0.59890243):
    data = load_p1100902()
    velocities = [0]
    for i, (t, x, y) in enumerate(data[1:]):
        (prev_x, prev_y) = data[i-1][1:]
        distance = (x-prev_x)
        delta_time = t - data[i-1][0]
        velocities.append(distance/delta_time)
    
    t = data[:,0]
    x = data[:,1]
    
    passes = detectPasses(t, x, velocities, obstacle_x)

    return t, x, velocities

def simulate_with_braking(braking_factor=0.1119893, obstacle_x = 0.59890243, steps = 600, timestep=0.016):
    """
    :return: False|int|tuple, int if passes are invalid
    """
    (ball_x, ball_y), polynom, track_end_x = tracker_1()
    
    ball = num_integ.Ball(mass=0.05, radius=0.0189/2, start_x=ball_x, start_y=ball_y)
    track = num_integ.Track(polynom, start_height=0.3, obstacle_height=0.238, end_height=0.3)
    
    integrator = num_integ.BallIntegrator(ball, track, braking_factor=braking_factor)
    integrator.timestep = timestep
    
    for step in range(steps):
        integrator.step()
    
    forceCalculator = num_integ.ForceCalculator(track, ball, integrator.time, integrator.position, integrator.velocity, integrator.acceleration)
    passChecker = num_integ.ObstaclePassChecker(obstacle_x)
    passes = detectPasses(integrator.time, integrator.position, integrator.velocity, obstacle_x)

    if len(passes) != 2:
        print(f"Invalid braking factor {braking_factor}, {len(passes)} passes")
        return len(passes)
    
    frictions, normalForces = forceCalculator.calculate()    
    for i, n in enumerate(normalForces):
        if n < 0:
            print(f"Invalid setup. Ball flies at force index {i}")
            return False
    
    return (integrator.time, integrator.position, integrator.velocity)

def find_error(measured_t, measured_x, calc_t, calc_x):
    """
    :return: float, the error
    """
    # 1: interpolate to match t values
    # 2: diff x values at interpolated values
    if len(measured_x) != len(calc_x):
        raise Exception(f"Must be same size! {len(measured_x)} vs {len(calc_x)}")
    return np.square(np.sum(np.square(calc_x - measured_x))/len(measured_x))
    

def find_close_match():
    (meas_t, meas_x, meas_v) = calculate_measured_values()
    braking_factor = 0.13 #0.111
    low_factor = 0.095
    high_factor = 0.4
    
    last_error = np.inf
    smallest_error = np.inf
    best_factor = braking_factor
    
    compare_size = 600
    frame_rate_seconds=0.01
    
    errors = []
    
    for run_number in range(30):
        print(f"Run {run_number}. Factor {braking_factor}" + "-"*20)
        simulated = simulate_with_braking(braking_factor=braking_factor, steps=compare_size-1, timestep=frame_rate_seconds)
        if isinstance(simulated, bool) and simulated == False:
            # Flight
            low_factor = braking_factor
            braking_factor = (high_factor + low_factor) / 2
        elif isinstance(simulated, int):
            # Invalid pass number
            passes = simulated
            print(f"Found {passes} passes with factor {braking_factor}. Range ({low_factor}, {high_factor})")
            if passes < 2:
                high_factor = braking_factor
            elif passes > 2:
                low_factor = braking_factor
            braking_factor = (high_factor + low_factor)/2
            print(f"Adjusted factors: {braking_factor} in range ({low_factor}, {high_factor})")
        else:
            # Normal case
            (calc_t, calc_x, calc_v) = simulated
            error = find_error(meas_t[:compare_size], meas_x[:compare_size], calc_t, calc_x)
            errors.append((braking_factor, error))
            
            if error < smallest_error:
                # New best case
                smallest_error = error
                best_factor = braking_factor
                print(f"Found better factor: {braking_factor} with error {error}")
            
            # Assuming error is in a valley, from previous graphed runs
            if error > last_error:
                # Error too far from best case, go back towards best
                braking_factor = (best_factor + braking_factor) / 2
            else:
                # Look around best case
                braking_factor = random.uniform(0.9, 1.1)*best_factor
            last_error = error
    
    errors = np.array(sorted(errors, key=lambda err: err[0]))
    plt.plot(errors[:,0], errors[:,1], label="Error")
    plt.legend()
    plt.show()
    
    print("-"*100)
    print(f"Best value found: {best_factor} (error={smallest_error})")
    # 0.11097230149082478 (error=4.3536731384222186e-07)
    # 0.11098230149082479 (error=3.340337453326927e-06) with 800 compare_size

find_close_match()
Run 0. Factor 0.13--------------------
Invalid braking factor 0.13, 1 passes
Found 1 passes with factor 0.13. Range (0.095, 0.4)
Adjusted factors: 0.1125 in range (0.095, 0.13)
Run 1. Factor 0.1125--------------------
Invalid braking factor 0.1125, 1 passes
Found 1 passes with factor 0.1125. Range (0.095, 0.13)
Adjusted factors: 0.10375000000000001 in range (0.095, 0.1125)
Run 2. Factor 0.10375000000000001--------------------
Found better factor: 0.10375000000000001 with error 0.002106042948863202
Run 3. Factor 0.11207187827794775--------------------
Invalid braking factor 0.11207187827794775, 1 passes
Found 1 passes with factor 0.11207187827794775. Range (0.095, 0.1125)
Adjusted factors: 0.10353593913897388 in range (0.095, 0.11207187827794775)
Run 4. Factor 0.10353593913897388--------------------
Run 5. Factor 0.10364296956948695--------------------
Run 6. Factor 0.10778739327779467--------------------
Found better factor: 0.10778739327779467 with error 0.0009688679819964324
Run 7. Factor 0.10369772769180852--------------------
Run 8. Factor 0.1057425604848016--------------------
Run 9. Factor 0.11099084401714228--------------------
Found better factor: 0.11099084401714228 with error 8.822935811007947e-07
Run 10. Factor 0.10313587498958733--------------------
Run 11. Factor 0.1070633595033648--------------------
Run 12. Factor 0.1202025285406901--------------------
Invalid braking factor 0.1202025285406901, 1 passes
Found 1 passes with factor 0.1202025285406901. Range (0.095, 0.11207187827794775)
Adjusted factors: 0.10760126427034505 in range (0.095, 0.1202025285406901)
Run 13. Factor 0.10760126427034505--------------------
Run 14. Factor 0.10882969421517562--------------------
Run 15. Factor 0.12066921286479099--------------------
Invalid braking factor 0.12066921286479099, 1 passes
Found 1 passes with factor 0.12066921286479099. Range (0.095, 0.1202025285406901)
Adjusted factors: 0.1078346064323955 in range (0.095, 0.12066921286479099)
Run 16. Factor 0.1078346064323955--------------------
Run 17. Factor 0.10941272522476889--------------------
Run 18. Factor 0.11172808289038938--------------------
Invalid braking factor 0.11172808289038938, 1 passes
Found 1 passes with factor 0.11172808289038938. Range (0.095, 0.12066921286479099)
Adjusted factors: 0.10336404144519469 in range (0.095, 0.11172808289038938)
Run 19. Factor 0.10336404144519469--------------------
Run 20. Factor 0.10717744273116848--------------------
Run 21. Factor 0.11503204015657845--------------------
Invalid braking factor 0.11503204015657845, 1 passes
Found 1 passes with factor 0.11503204015657845. Range (0.095, 0.11172808289038938)
Adjusted factors: 0.10501602007828922 in range (0.095, 0.11503204015657845)
Run 22. Factor 0.10501602007828922--------------------
Run 23. Factor 0.10800343204771576--------------------
Run 24. Factor 0.12196334653509773--------------------
Invalid braking factor 0.12196334653509773, 1 passes
Found 1 passes with factor 0.12196334653509773. Range (0.095, 0.11503204015657845)
Adjusted factors: 0.10848167326754887 in range (0.095, 0.12196334653509773)
Run 25. Factor 0.10848167326754887--------------------
Run 26. Factor 0.11469316578600632--------------------
Invalid braking factor 0.11469316578600632, 1 passes
Found 1 passes with factor 0.11469316578600632. Range (0.095, 0.12196334653509773)
Adjusted factors: 0.10484658289300317 in range (0.095, 0.11469316578600632)
Run 27. Factor 0.10484658289300317--------------------
Run 28. Factor 0.10791871345507273--------------------
Run 29. Factor 0.10400125767461812--------------------
----------------------------------------------------------------------------------------------------
Best value found: 0.11099084401714228 (error=8.822935811007947e-07)

Animating the simulation

In [19]:
# Curve with obstacle animation
def realObstacleCurveAnimation():
    fig, ax = plt.subplots()

    # Background path using the curve
    (ball_x, ball_y), parabola, track_end_x = tracker_1()
    
    plt.xlim((0, track_end_x))
    steps = 100
    x = np.linspace(0, track_end_x, steps)
    y = np.polyval(parabola, x)
    plt.plot(x, y, label="Track")
    plt.legend()
    
    # Animated line
    ax.set_xlim((0, track_end_x))
    ax.set_ylim((0, 0.33))
    line, = ax.plot([], [], linestyle='None', marker="o", label="Frame: 0")
    ax.legend()
    
    # Animation settings
    framerate = 1/25
    frame_count = 150
    print(f"Total duration of animation is {framerate*frame_count}s")

    obstacle_x = 0.59890243
    #print(f"Roots at {np.roots(np.polyder(polynom))}")
    print(f"Obstacle at {obstacle_x}")
    
    ball = num_integ.Ball(mass=0.05, radius=0.0189/2, start_x=ball_x, start_y=ball_y)
    track = num_integ.Track(parabola, start_height=0.3, obstacle_height=0.238, end_height=0.3)
    
    integrator = num_integ.BallIntegrator(ball, track, braking_factor=0.11097230149082478) # Found later
    integrator.timestep = framerate
    
    def init():
        line.set_data([], [])
        return (line,)
        
    x_pos = [0]
    y_pos = [np.polyval(parabola, x_pos[0])]
        
    def animate(frame):
        integrator.step()
        x = integrator.position[-1]
        y = np.polyval(parabola, x)
        line.set_data(x, y)
        line.set_label(f"Frame {frame}, t={np.round(frame*framerate, 1)}. pos=({round(x,1)}, {round(y,1)}) ")
        ax.legend()
        return (line,) # Return all updated artists
    
    anim = animation.FuncAnimation(fig, animate, init_func=init,
                                   frames=frame_count, interval=framerate*1000, 
                                   blit=True)
    rc('animation', html='jshtml')
    return anim


def showAnimation():
    anim = realObstacleCurveAnimation()
    save_vid = False
    if save_vid:
        # Set up formatting for the movie files
        Writer = animation.writers['ffmpeg']
        writer = Writer(fps=25, metadata=dict(artist='Kristian'), bitrate=1800)
        anim.save('out/2-real-track-data.mp4', writer=writer)
        print("Saved!")
    return anim

if run_slow:
    showAnimation()
Total duration of animation is 6.0s
Obstacle at 0.59890243
Saved!
Out[19]:


Once Loop Reflect